﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Diagnostics;
using Microsoft.Xna.Framework.Graphics;

namespace Brain.Logic3D.PathFinding
{
    public class Map3D
    {
        public static float Epsilon = 0.0001f;

        public PathNode3D[] Nodes;

        public Map3D(int tesselationFactor, Vector3[] vertices)
        {
            Load(tesselationFactor, vertices);
        }
        public Map3D(int tesselationFactor, TerrainWorldObject world)
        {
            Load(tesselationFactor, world.Triangles);
        }

        private void Load(int tesselationFactor, Vector3[] vertices)
        {
            if (tesselationFactor < 1)
            {
                throw new Exception("Tesselation Factor cannot be less than 1");
            }

            // * (tesselationFactor - 1) * 4
            Triangle[] Triangles = new Triangle[vertices.Length / 3];
            if (tesselationFactor == 1)
            {
                Nodes = new PathNode3D[Triangles.Length];
            }
            for (int i = 0; i < vertices.Length - 2; i += 3)
            {
                Triangle tri = new Triangle();

                int k = i / 3;

                Vector3 v1 = vertices[i];
                Vector3 v2 = vertices[i + 1];
                Vector3 v3 = vertices[i + 2];

                tri.Vertex1 = v1;
                tri.Vertex2 = v2;
                tri.Vertex3 = v3;

                tri.Normal = TriangleUtil.Normal(v1, v2, v3);

                Triangles[k] = tri;

                if (tesselationFactor == 1)
                {
                    PathNode3D node = new PathNode3D();
                    node.Triangle = tri;
                    Nodes[k] = node;
                }
            }
            Load(tesselationFactor, Triangles);
        }
        private void Load(int tesselationFactor, Triangle[] Triangles)
        {
            if (tesselationFactor == 1 && Nodes == null)
            {
                Nodes = new PathNode3D[Triangles.Length];

                for (int i = 0; i < Triangles.Length; i++)
                {
                    Triangle tri = Triangles[i];
                    PathNode3D node = new PathNode3D();
                    node.Triangle = tri;
                    Nodes[i] = node;
                }
            }
            if (tesselationFactor != 1)
            {
                for (int i = 0; i < tesselationFactor - 1; i++)
                {
                    Triangle[] news = TriangleUtil.Tesselate(Triangles);
                    Triangles = news;
                }

                Nodes = new PathNode3D[Triangles.Length];

                for (int i = 0; i < Triangles.Length; i++)
                {
                    Triangle tri = Triangles[i];
                    PathNode3D node = new PathNode3D();
                    node.Triangle = tri;
                    Nodes[i] = node;
                }
            }

            for (int i = 0; i < Nodes.Length; i++)
            {
                PathNode3D n1 = Nodes[i];
                n1.Index = i;
                Triangle tri1 = n1.Triangle;

                for (int k = 0; k < Nodes.Length; k++)
                {
                    PathNode3D n2 = Nodes[k];
                    bool V1Equal = false;
                    bool V2Equal = false;
                    bool V3Equal = false;

                    Triangle tri2 = n2.Triangle;
                    
                    if (tri2 != tri1)
                    {
                        if (Vector3Util.Equal(tri1.Vertex1, tri2.Vertex1, Epsilon) ||
                            Vector3Util.Equal(tri1.Vertex1, tri2.Vertex2, Epsilon) ||
                            Vector3Util.Equal(tri1.Vertex1, tri2.Vertex3, Epsilon))
                        {
                            V1Equal = true;
                        }
                        if (Vector3Util.Equal(tri1.Vertex2, tri2.Vertex1, Epsilon) ||
                            Vector3Util.Equal(tri1.Vertex2, tri2.Vertex2, Epsilon) ||
                            Vector3Util.Equal(tri1.Vertex2, tri2.Vertex3, Epsilon))
                        {
                            V2Equal = true;
                        }
                        if (Vector3Util.Equal(tri1.Vertex3, tri2.Vertex1, Epsilon) ||
                            Vector3Util.Equal(tri1.Vertex3, tri2.Vertex2, Epsilon) ||
                            Vector3Util.Equal(tri1.Vertex3, tri2.Vertex3, Epsilon))
                        {
                            V3Equal = true;
                        }

                        if (V1Equal && V2Equal)
                        {
                            n1.SideV1V2 = tri2;
                            n1.PathV1V2 = n2;
                        }
                        if (V2Equal && V3Equal)
                        {
                            n1.SideV2V3 = tri2;
                            n1.PathV2V3 = n2;
                        }
                        if (V1Equal && V3Equal)
                        {
                            n1.SideV1V3 = tri2;
                            n1.PathV1V3 = n2;
                        }
                    }
                }
            }
        }
    }
}
